#include "rplidar_process_core.h"

RplidarProcessCore::RplidarProcessCore(ros::NodeHandle &nh)
{
    ros::Subscriber sub = nh.subscribe("/scan", 10, &RplidarProcessCore::Lidar_Callback,this);
    pub_obstacle_ = nh.advertise<sensor_msgs::PointCloud2>("/obstable_positions_topic", 10);

    ros::spin();
};

RplidarProcessCore::~RplidarProcessCore(){};

void RplidarProcessCore::Lidar_Callback(const sensor_msgs::LaserScan::ConstPtr &scan)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    cloud->header.frame_id = scan->header.frame_id;
    cloud->header.stamp = pcl_conversions::toPCL(scan->header.stamp);
    //pcl::PointXYZ point;
    int count = scan->scan_time / scan->time_increment;
    ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count);
    ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max));
  
    for(int i = 0; i < count; i++) {
        //float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
        float degree = scan->angle_min + scan->angle_increment * i;

        //ROS_INFO(": [%f, %f]", degree, scan->ranges[i]);

        pcl::PointXYZ point;

        // if (scan->ranges[i] < scan->range_min)
        // {
        //   // Invalid range
        //   point.x = std::numeric_limits<float>::quiet_NaN();
        //   point.y = std::numeric_limits<float>::quiet_NaN();
        //   point.z = std::numeric_limits<float>::quiet_NaN();
        //   cloud->is_dense = false;
        // }
        // else if (scan->ranges[i] > scan->range_max)
        // {
        //   // "Invalid" range
        //   point.x = 1000000 * std::cos(degree); // std::numeric_limits<float>::infinity();
        //   point.y = 1000000 * std::sin(degree); //std::numeric_limits<float>::infinity();
        //   point.z = 0;  // TODO: FIX?
        //   cloud->is_dense = false;
        // }
        // else
        // {
        //     point.x = scan->ranges[i] * std::cos(degree);
        //     point.y = scan->ranges[i] * std::sin(degree);
        //     point.z = 0;

        //     ROS_INFO(": (%f, %f, %f)", point.x, point.y, point.z);

        // }

        //cloud->points.push_back(point);
        if(scan->ranges[i] < 30)
        {
            point.x = scan->ranges[i] * cos(degree);
            point.y = scan->ranges[i] * sin(degree);
            point.z = 0;
            cloud->push_back(point);
        }
    }
    ROS_INFO(": cloud size %d", cloud->size());

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (cloud);

    std::vector<pcl::PointIndices> indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> euclid;
    euclid.setInputCloud(cloud);
    euclid.setClusterTolerance(0.05);
    euclid.setMinClusterSize(3);
    euclid.setMaxClusterSize(80);
    euclid.setSearchMethod(tree);
    euclid.extract(indices);

    ROS_INFO(": %d", indices.size());

    pcl::PointXYZ p;
    
    pcl::PointCloud<pcl::PointXYZ> obstacle_positions_;

    obstacle_positions_.clear();

    for (size_t i = 0; i < indices.size(); i++)
    {
        p.x = 0;
        p.y = 0;
        p.z = 0;

        for (auto pit = indices[i].indices.begin(); pit != indices[i].indices.end(); ++pit)
        {
            p.x += cloud->points[*pit].x;
            p.y += cloud->points[*pit].y;
        }
        if(indices[i].indices.size() > 0)
        {
            p.x /= indices[i].indices.size();
            p.y /= indices[i].indices.size();
            p.z = 0;
        }
        obstacle_positions_.points.push_back(p);

        ROS_INFO(": obstacle position: (%f, %f, %f)", p.x, p.y, p.z);
    }

    ROS_INFO(": obstacle size: %d" ,obstacle_positions_.points.size());
    sensor_msgs::PointCloud2 obstacle_positions_out;
    pcl::toROSMsg(obstacle_positions_, obstacle_positions_out);
    obstacle_positions_out.header.frame_id = "laser";
    
    pub_obstacle_.publish(obstacle_positions_out);

};
